/*
 * usercode.c
 *
 *  Created on: 2025��8��13��
 *      Author: Bernard
 */
#include "usercode.h"

Cars car={0};
/********************引脚配置*******************
*   A_FOC   33.4 - 33.9 / adc A1 A2 A0
*   B_FOC   22.0 - 22.3 + 21.2 + 11.2 / adc A3 A4 A5
*   IMU660RA  20.0 + 10.1-10.3
*   TOF    P33_0 P33_1 20.10 
*   IPS    15.0-15.5 + 11.9-11.12 + 13.0-13.3
*   MT9    00.0 02.2 02.3
*   磁编    20.11-20.15
*/
/**************************************接口*************************************************************/
void Car_get_speed(Cars *car)
{
    car->dynamic.left_speed =  FOC_Get_ShaftSpeed(&B_FOC, 1);
    car->dynamic.right_speed =  FOC_Get_ShaftSpeed(&A_FOC, 1);
    car->dynamic.speed = (car->dynamic.left_speed + car->dynamic.right_speed) / 2.0f;
}

void Car_get_angle(Cars *car)
{
    car->dynamic.angle_pitch = eulerAngle.pitch; //俯仰角
    car->dynamic.angle_roll = eulerAngle.roll; //横滚角
    car->dynamic.angle_yaw = eulerAngle.yaw; //偏航角
}

void Car_get_gyro(Cars *car)
{
    car->dynamic.gyro_x = imu_data.gyro_x;
    car->dynamic.gyro_y = imu_data.gyro_y; 
    car->dynamic.gyro_z = imu_data.gyro_z; 
}

/**************************************功能*************************************************************/
/*
*除FOC以外的整车硬件初始化
**/
void Car_Hardware_Init(void)
{
     ips200_init (IPS200_TYPE_SPI);  //IPS
     mt9v03x_init ();   // MT9V03X 
     dl1b_init();     //TOF
     imu660ra_init();    // IMU660RA
}
void Car_Software_Init(Cars *car)
{
    car->setparam.zero_angle = -78.0f;
    car->setparam.mode = Stop;
    car->setparam.hight = low;
    car->scheduler_count = 0;
}
uint8_t Car_ready(Cars *car)
{
    if(car->scheduler_count == 0)
    {
        return 1;
    }
}
/*************************************整车逻辑程序机****************************************************/
uint8_t Car_Run(Cars *car)
{
    switch (car->scheduler_count)
    {
    case 0:/*未启动*/
        {
            if(Car_ready(car))
            {
                car->setparam.mode = Run;
                car->setparam.hight = low;
                car->scheduler_count = 1;
            } 
        }
        break;
    case 1:/*巡线*/
        {
            
        }
        break;
    default:
        break;
    }
}


/*********************************************************************************************************/
void cpu0_init(void)
{
    menc15a_init();
    A_FOC_Par_Init();
    B_FOC_Par_Init();
    A_FOC_Hardware_Init();
    B_FOC_Hardware_Init();
    ControllerInit();
}
void cpu1_init(void)
{
    Car_Hardware_Init();
    Car_Software_Init(&car);
    Scheduler_Setup();
}
void cpu2_init(void)
{
    Scheduler_Run();
}
void cpu0_while(void)
{
    ControllerRun();
}
void cpu1_while(void)
{
    
}
void cpu2_while(void)
{
    
}





